#ifndef FEHLBERGEMBEDDEDRK_HPP
#define FEHLBERGEMBEDDEDRK_HPP

#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>
#include "norm.hpp"
#include "Function.hpp"

using namespace std;

class FehlbergEmbeddedRk : public TimeIntegrator
{
private:
    int n_steps, N = 6;
    double T, mu;
    vector<double> value;
    vector<vector<double>> result;
    Norm norm;

public:
    FehlbergEmbeddedRk() {}
    FehlbergEmbeddedRk(int n_, double time, vector<double> Val) : n_steps(n_), T(time), value(Val) {}
    ~FehlbergEmbeddedRk() {}
    void solver();
    void error();
};

void FehlbergEmbeddedRk::solver()
{
    ofstream outfile;
    outfile.open("output.m");
    outfile << "points = [";
    double k = 1.0 * T / n_steps;
    vector<double> U = value;
    result.push_back(U);
    auto epsilon = 1e-6;
    double t = 0;

    while (t < T)
    {
        k = min(k, T - t);
        auto y1 = f(U, t);
        auto y2 = f(U + k / 4 * y1, t + k / 4);
        auto y3 = f(U + k * (3.0 / 32 * y1 + 9.0 / 32 * y2), t + 3.0 / 8 * k);
        auto y4 = f(U + k * (1932.0 / 2197 * y1 - 7200.0 / 2197 * y2 + 7296.0 / 2197 * y3), 12.0 / 13 * k + t);
        auto y5 = f(U + k * (439.0 / 216 * y1 - 8.0 * y2 + 3680.0 / 513 * y3 - 845.0 / 4104 * y4), t + k);
        auto y6 = f(U + k * (-8.0 / 27 * y1 + 2.0 * y2 - 3544.0 / 2565 * y3 + 1859.0 / 4104 * y4 - 11.0 / 40 * y5), t + 0.5 * k);
        auto U1 = U + k * (25.0 / 216 * y1 + 1408.0 / 2565 * y3 + 2197.0 / 4104 * y4 - 0.2 * y5);
        auto U2 = U + k * (16.0 / 135 * y1 + 6656.0 / 12825 * y3 + 28561.0 / 56430 * y4 - 9.0 / 50 * y5 + 2.0 / 55 * y6);
        double E = norm.inftynorm(U1 - U2) / epsilon;
        if (E <= 1)
        {
            U = U2;
            outfile << U << endl;
            result.push_back(U);
            t += k;
        }
        k = 0.84 * pow(E, -1.0 / 4) * k;
    }
    outfile << "];" << endl;
    outfile << "scatter(points(:,1), points(:,2),\".\");" << endl;
    outfile.close();
}

void FehlbergEmbeddedRk::error()
{
    int n = result.size();
    // cout << result[n - 1] << endl;
    // cout << result[0] << endl;
    double error = norm.Inftynorm(result[n - 1] - result[0]);
    cout << error << endl;
}

TimeIntegrator* createFehlberg(int n_, int p_, double T_, vector<double> value_) { return new FehlbergEmbeddedRk(n_, T_, value_); }
#endif